#!/bin/bash

# RobotOne传感器节点测试脚本
# 作者: AI进化论-花生
# 日期: 2024
# 版本: 0.0.0
# 许可证: MIT

set -euo pipefail

# 颜色定义
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
NC='\033[0m' # No Color

# 日志函数
log_info() {
    echo -e "${BLUE}[INFO]${NC} $1"
}

log_success() {
    echo -e "${GREEN}[SUCCESS]${NC} $1"
}

log_warning() {
    echo -e "${YELLOW}[WARNING]${NC} $1"
}

log_error() {
    echo -e "${RED}[ERROR]${NC} $1"
}

# 检查ROS2环境
check_ros2_environment() {
    log_info "检查ROS2环境..."
    
    if ! command -v ros2 &> /dev/null; then
        log_error "ROS2未安装或未正确配置"
        return 1
    fi
    
    if [ -z "${ROS_DISTRO:-}" ]; then
        log_error "ROS_DISTRO环境变量未设置"
        return 1
    fi
    
    log_success "ROS2环境检查通过: $ROS_DISTRO"
    return 0
}

# 编译项目
build_project() {
    log_info "编译RobotOne项目..."
    
    if ! colcon build --packages-select robotone; then
        log_error "项目编译失败"
        return 1
    fi
    
    log_success "项目编译成功"
    return 0
}

# 设置环境
setup_environment() {
    log_info "设置ROS2环境..."
    
    if [ ! -f "install/setup.bash" ]; then
        log_error "install/setup.bash文件不存在，请先编译项目"
        return 1
    fi
    
    source install/setup.bash
    log_success "环境设置完成"
    return 0
}

# 测试节点启动
test_node_startup() {
    log_info "测试传感器节点启动..."
    
    # 启动节点（后台运行）
    ros2 run robotone sensor_node &
    local node_pid=$!
    
    # 等待节点启动
    sleep 3
    
    # 检查节点是否在运行
    if ! kill -0 $node_pid 2>/dev/null; then
        log_error "节点启动失败"
        return 1
    fi
    
    # 检查节点是否在节点列表中
    if ! ros2 node list | grep -q "sensor_node"; then
        log_error "节点未在ROS2节点列表中"
        kill $node_pid 2>/dev/null || true
        return 1
    fi
    
    # 停止节点
    kill $node_pid 2>/dev/null || true
    wait $node_pid 2>/dev/null || true
    
    log_success "节点启动测试通过"
    return 0
}

# 测试launch文件
test_launch_file() {
    log_info "测试launch文件..."
    
    # 启动launch文件（后台运行）
    timeout 5 ros2 launch robotone sensor.launch.py &
    local launch_pid=$!
    
    # 等待launch文件启动
    sleep 2
    
    # 检查节点是否在运行
    if ! ros2 node list | grep -q "sensor_node"; then
        log_error "Launch文件启动失败"
        kill $launch_pid 2>/dev/null || true
        return 1
    fi
    
    # 等待launch文件完成
    wait $launch_pid 2>/dev/null || true
    
    log_success "Launch文件测试通过"
    return 0
}

# 测试主题发布
test_topic_publishing() {
    log_info "测试主题发布..."
    
    # 启动节点（后台运行）
    ros2 run robotone sensor_node &
    local node_pid=$!
    
    # 等待节点启动
    sleep 3
    
    # 检查主题是否存在
    if ! ros2 topic list | grep -q "sensor_data"; then
        log_error "sensor_data主题不存在"
        kill $node_pid 2>/dev/null || true
        return 1
    fi
    
    # 监听主题消息（5秒）
    log_info "监听主题消息..."
    if timeout 5 ros2 topic echo /sensor_data --once; then
        log_success "主题发布测试通过"
    else
        log_warning "未收到主题消息，但主题存在"
    fi
    
    # 停止节点
    kill $node_pid 2>/dev/null || true
    wait $node_pid 2>/dev/null || true
    
    return 0
}

# 测试参数配置
test_parameter_configuration() {
    log_info "测试参数配置..."
    
    # 使用自定义参数启动launch文件
    timeout 5 ros2 launch robotone sensor.launch.py publish_rate:=5.0 queue_size:=20 &
    local launch_pid=$!
    
    # 等待启动
    sleep 2
    
    # 检查节点参数
    if ros2 param get /sensor_node publish_rate | grep -q "5.0"; then
        log_success "参数配置测试通过"
    else
        log_warning "参数配置可能有问题"
    fi
    
    # 等待launch文件完成
    wait $launch_pid 2>/dev/null || true
    
    return 0
}

# 主测试函数
main() {
    log_info "开始RobotOne传感器节点测试..."
    
    local test_count=0
    local passed_count=0
    
    # 执行所有测试
    tests=(
        "check_ros2_environment"
        "build_project"
        "setup_environment"
        "test_node_startup"
        "test_launch_file"
        "test_topic_publishing"
        "test_parameter_configuration"
    )
    
    for test in "${tests[@]}"; do
        test_count=$((test_count + 1))
        log_info "执行测试 $test_count/${#tests[@]}: $test"
        
        if $test; then
            passed_count=$((passed_count + 1))
        else
            log_error "测试失败: $test"
        fi
        
        echo ""
    done
    
    # 输出测试结果
    log_info "测试完成!"
    log_info "通过: $passed_count/$test_count"
    
    if [ $passed_count -eq $test_count ]; then
        log_success "所有测试通过! 🎉"
        return 0
    else
        log_error "部分测试失败"
        return 1
    fi
}

# 脚本入口
if [[ "${BASH_SOURCE[0]}" == "${0}" ]]; then
    main "$@"
fi
